package org.hxzon.cyclone.collide.fine;

import org.hxzon.cyclone.Util;
import org.hxzon.cyclone.Vector3;

public class IntersectionTests {

    public static boolean sphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane) {
        // Find the distance from the origin
        double ballDistance = plane.direction.scalarProduct(sphere.getAxis(3)) - sphere.radius;

        // Check for the intersection
        return ballDistance <= plane.offset;
    }

    public static boolean sphereAndSphere(CollisionSphere one, CollisionSphere two) {
        // Find the vector between the objects
        Vector3 midline = one.getAxis(3).minus(two.getAxis(3));

        // See if it is large enough.
        return midline.squareMagnitude() //
        < (one.radius + two.radius) * (one.radius + two.radius);
    }

    public static boolean boxAndBox(CollisionBox one, CollisionBox two) {
        // Find the vector between the two centres
        Vector3 toCentre = two.getAxis(3).minus(one.getAxis(3));

        return (
        // Check on box one's axes first
        overlapOnAxis(one.getAxis(0), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(1), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(2), toCentre, one, two) //

                // And on two's
                && overlapOnAxis(two.getAxis(0), toCentre, one, two) //
                && overlapOnAxis(two.getAxis(1), toCentre, one, two) //
                && overlapOnAxis(two.getAxis(2), toCentre, one, two) //

                // Now on the cross products
                && overlapOnAxis(one.getAxis(0).vectorProduct(two.getAxis(0)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(0).vectorProduct(two.getAxis(1)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(0).vectorProduct(two.getAxis(2)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(1).vectorProduct(two.getAxis(0)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(1).vectorProduct(two.getAxis(1)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(1).vectorProduct(two.getAxis(2)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(2).vectorProduct(two.getAxis(0)), toCentre, one, two) //
                && overlapOnAxis(one.getAxis(2).vectorProduct(two.getAxis(1)), toCentre, one, two) //
        && overlapOnAxis(one.getAxis(2).vectorProduct(two.getAxis(2)), toCentre, one, two));
    }

    public static boolean overlapOnAxis(Vector3 axis, Vector3 toCentre, //
            CollisionBox one, CollisionBox two) {
        return overlapOnAxis(one, two, axis, toCentre);
    }

    /**
     * Does an intersection test on an arbitrarily aligned box and a
     * half-space.
     *
     * The box is given as a transform matrix, including
     * position, and a vector of half-sizes for the extend of the
     * box along each local axis.
     *
     * The half-space is given as a direction (i.e. unit) vector and the
     * offset of the limiting plane from the origin, along the given
     * direction.
     */
    public static boolean boxAndHalfSpace(CollisionBox box, CollisionPlane plane) {
        // Work out the projected radius of the box onto the plane direction
        double projectedRadius = transformToAxis(box, plane.direction);

        // Work out how far the box is from the origin
        double boxDistance = plane.direction.scalarProduct(box.getAxis(3)) - projectedRadius;

        // Check for the intersection
        return boxDistance <= plane.offset;
    }

    public static double transformToAxis(CollisionBox box, Vector3 axis) {
        return box.halfSize.x * Util.realAbs(axis.scalarProduct(box.getAxis(0))) //
                + box.halfSize.y * Util.realAbs(axis.scalarProduct(box.getAxis(1))) //
                + box.halfSize.z * Util.realAbs(axis.scalarProduct(box.getAxis(2)));
    }

    /**
     * This function checks if the two boxes overlap
     * along the given axis. The final parameter toCentre
     * is used to pass in the vector between the boxes centre
     * points, to avoid having to recalculate it each time.
     */
    public static boolean overlapOnAxis(CollisionBox one, CollisionBox two, //
            Vector3 axis, Vector3 toCentre) {
        // Project the half-size of one onto axis
        double oneProject = transformToAxis(one, axis);
        double twoProject = transformToAxis(two, axis);

        // Project this onto the axis
        double distance = Util.realAbs(toCentre.scalarProduct(axis));

        // Check for overlap
        return (distance < oneProject + twoProject);
    }
}
